//
//  benches.c
//  KFilter
//
//  Created by Kaiwen Gu on 6/21/13.
//  Copyright (c) 2013 com.mika. All rights reserved.
//

#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include "ahrs.h"
#include "erza_matrix_lib.h"
#include "benches.h"

#define B_ROW 20
#define B_COL 20
#define B_ITER 10000

typedef struct INSInput_TypeDef {
	float values[11];

} INSInput_TypeDef;

INSInput_TypeDef INSInputArray[3000];
int INSInputArrayCount = 0;

int main(void) {

	AHRSInit();

/*
	for (int i = 0; i < 5; i++) {


		processInertialSensors(0.004, 0.004, 0.004, 0.0, 0.0, 1.0);
		processAccelerometerObservation(0.01, 0.01, 0.34);
		printf("----------------------------------------------------------------------------\n");
			printf("ITERATION: %d\n", i + 1);
			printf("----------------------------------------------------------------------------\n");
	}
*/
	procesDataUKF();
	return 1;

	/*
	 for (int i = 0; i < 500; i++) {
	 //BENCH_START
	 processInertialSensors(0, 0, 0.5, 0, 0, 1);
	 processAccelerometerObservation(0, 0, 1);
	 processBarometricPressureObservation(0.5);

	 //BENCH_END

	 printf("----------------------------------------------------------------------------\n");
	 printf("ITERATION: %d\n", i);
	 printf("----------------------------------------------------------------------------\n");
	 //processBarometricPressureObservation(0.9);
	 }
	 */

	//	BENCH_START
	//processInertialSensors(0, 0, 0.5, 0, 0, 1);
	//processAccelerometerObservation(0, 0, 1);
	//BENCH_END
}

void procesDataUKF(void) {
	readFile();

	for (int i = 0; i < 6000; i++) {
		float ax = INSInputArray[i % INSInputArrayCount].values[0];
		float ay = INSInputArray[i % INSInputArrayCount].values[1];
		float az = INSInputArray[i % INSInputArrayCount].values[2];

		float gx = INSInputArray[i % INSInputArrayCount].values[3];
		float gy = INSInputArray[i % INSInputArrayCount].values[4];
		float gz = INSInputArray[i % INSInputArrayCount].values[5];

		//float mx = INSInputArray[i % INSInputArrayCount].values[6];
		//float my = INSInputArray[i % INSInputArrayCount].values[7];
		//float mz = INSInputArray[i % INSInputArrayCount].values[8];

		//printf("%f %f %f %f %f %f \n", ax, ay, az, gx, gy, gz);
		processInertialSensors(gx, gy, gz, ax, ay, az);
		processAccelerometerObservation(ax, ay, az);
		processOutput(AHRS_OUT_QUATERNION);
		processOutput(10);

	}
}

void readFile(void) {
	char * token;
	float convertedToken;
	int bufferIndex = 0;
	INSInput_TypeDef INSInput;

	static const char filename[] = "/Users/kaiwen_gu/Dropbox/robotics/eWorkspace/ukf/src/mika2.dat";
	FILE *file = fopen(filename, "r");
	if (file != NULL ) {
		char line[256]; /* or other suitable maximum line size */
		while (fgets(line, sizeof line, file) != NULL ) /* read a line */
		{
			bufferIndex = 0;
			token = strtok(line, ",");
			while (token != NULL ) {
				convertedToken = atof(token);
				INSInput.values[bufferIndex++] = convertedToken;
				token = strtok(NULL, ",");
			}
			INSInputArray[INSInputArrayCount++] = INSInput;
		}
		fclose(file);
	} else {
		perror(filename); /* why didn't the file open? */
	}
	return;
}

